#include "VanishingPointOracle.h"

namespace abliefern {
	
	VanishingPointOracle::VanishingPointOracle(std::string theFileName, std::string thePathName){
		_myIsInited = false;
		_myIsFinishedCompletely = false;
		_myHasNoMoreProposals = false;

		_myHasProposal = false;
		
		_myLineThreshold = 15;
		_myLineThresholdSquared = _myLineThreshold * _myLineThreshold;

		_myScale = 0.09;

		
		_myFileName = theFileName;
		_myPathName = thePathName;
		
		
		

		
	}
	
	
	void VanishingPointOracle::threadedFunction(){
		printf("Loading.\n");
		loadImage();
		printf("Processing.\n");

		processImage();
		
		//filterLines();
		
		
		while( _myIsInited == false ){
			if( lock() ){
				_myIsInited = true;
				
				unlock();
				
			}
		}
		

	}
	
	
	void VanishingPointOracle::start(){
		startThread(true, false);   // blocking, verbose
	}
	
	
	void VanishingPointOracle::kill(){
		stopThread();
	}
	
	
	VanishingPointOracle::~VanishingPointOracle(){
		printf("### VAPOR destructor called");
	}
	
	void VanishingPointOracle::update(){
		if(hasFinishedLoading()){
			if(_myDisplayImage.getWidth() < 10){
				printf("### Updating view image \n");
				_myDisplayImage.setFromPixels((unsigned char*)_myOriginalImage.getPixels(), _myOriginalImage.width, _myOriginalImage.height, _myOriginalImage.type, TRUE);
				_myDisplayImage.resize(_myOriginalImage.width/2, _myOriginalImage.height/2);

			}
			
			if(!_myHasProposal && !_myIsFinishedCompletely && !_myHasNoMoreProposals){
				proposeNewVanishingPoint();
			}
		} else {
			
		}
	}
	
	void VanishingPointOracle::draw(){
		ofSetColor(255,255,255);
		
		
		float myXOffset = ofGetWidth() * 0.5 - _myDisplayImage.width * _myScale * 0.5;
		float myYOffset = ofGetHeight() * 0.5 - _myDisplayImage.height * _myScale * 0.5;
		
		if(hasFinishedLoading()){
			ofDrawBitmapString(_myFileName , 20, 20);


			_myDisplayImage.draw(myXOffset, myYOffset,_myDisplayImage.width * _myScale *2 , _myDisplayImage.height * _myScale*2);

		} else {
			ofDrawBitmapString("Loading " + _myFileName , 20, 20);
			
		}

		
		
		if(_myHasProposal){
			
			ofSetColor(0, 128, 0);
			
			
			/* Draw lines */
			for(int i = 0; i < _mySupportLines.size(); i++){
				Segment* myLine = _mySupportLines.at(i);
				
				float diffX = myLine->x1 - myLine->x2;
				float diffY = myLine->y1 - myLine->y2;
				
				
				float x1 = myLine->x2 + diffX  *70;
				float y1 = myLine->y2 + diffY * 70;
				
				
				float x0 = myLine->x1 - diffX * 70;
				float y0 = myLine->y1 - diffY * 70;
				
				
				ofLine(x0*_myScale + myXOffset, y0 *_myScale  + myYOffset, x1 *_myScale  + myXOffset, y1 *_myScale + myYOffset);
				
				
			}
			
		}
		
		
		for (int i = 0; i< _mySelection.size(); i++){
			VanishingPoint* myPoint = &_mySelection.at(i);
			ofSetColor(255, 0,0);
			
			ofCircle(myPoint->x * _myScale + myXOffset, myPoint->y * _myScale + myYOffset, 5);
		}

	}

	
	


	
	bool VanishingPointOracle::hasFinishedCompletely(){
		return _myIsFinishedCompletely;
	}
	
	
	bool VanishingPointOracle::hasFinishedLoading(){
		return _myIsInited;
	}
	
	
	void VanishingPointOracle::removeCurrentSupportLines(){
		/* Remove this points support lines */
		
		
		
		for(int i = 0; i < _mySupportLines.size(); i++){
				
				
				std::vector<Segment*>::iterator myNewEnd;
				myNewEnd = remove(_myRemainingLines.begin(), _myRemainingLines.end(), _mySupportLines.at(i) );
				_myRemainingLines.erase(myNewEnd, _myRemainingLines.end());

			
		}

		
			
		_mySupportLines.clear();
		
	}
	
	
	void VanishingPointOracle::acceptVanishingPoint(){
		if(_myHasProposal){
			_mySelection.push_back(_myVPProposal);
			
			removeCurrentSupportLines();
			
			
		} 
		
		_myHasProposal = false;
	}
	
	
	void VanishingPointOracle::denyVanishingPoint(){
		if(_myHasProposal){

			_myIgnores.push_back(_myVPProposal);
		}
		
		_myHasProposal = false;
	}
	
	
	void VanishingPointOracle::ignoreVanishingPoint(){
		if(_myHasProposal){

			_myIgnores.push_back(_myVPProposal);
			removeCurrentSupportLines();
		}
		
		_myHasProposal = false;
	}
	

	
	void VanishingPointOracle::finishDetection(){
		
		_myIsFinishedCompletely = true;
		
	}
	
	
	
	void VanishingPointOracle::filterLines(){
		
		int myFilterCounter = 0;
		int myTotalCount = _myLines.size();

		
		/* Iterate through our list and remove filtered lines */
		std::vector<Segment>::iterator myIterator = _myLines.begin();
		
		while (myIterator != _myLines.end())
		{
			bool myDoFilter = filterLine((*myIterator).angle());
			
			if (myDoFilter)	{
				myFilterCounter++;
				_myLines.erase(myIterator++); 
			}
			else {
				++myIterator;
			}
		}
		
		
		printf("### Filtered %d of %d lines in total. So now there are %d.\n", myFilterCounter, myTotalCount, _myLines.size());
	}
	
	

	
	void VanishingPointOracle::proposeNewVanishingPoint(){
		
		
		/* Clear */
		
		_mySupportLines.clear();
		
		ofSeedRandom(); // new random seed
		
		Segment* myLine1;
		Segment* myLine2;
		
		int myCount = 15000;
		
		Point2f* myBestPoint = NULL;
		Point2f* myPoint = NULL;
		
		int bMsupp = 9999999, tempMsupp;
		
		std::vector<abliefern::Segment*> mySupports;
		
		std::vector<abliefern::Segment*> mySupports2;
		
		
		
		while ((myCount--) > 0)
		{
			int myNumber1 = randomInt(_myRemainingLines.size());
			int myNumber2 = randomInt(_myRemainingLines.size());
			
			if (myNumber1 == myNumber2)
			{
				continue;
			}
			
			myLine1 = _myRemainingLines.at(myNumber1);
			myLine2 = _myRemainingLines.at(myNumber2);
			
			myPoint = myLine1->intersect(myLine2);
			
			if (myPoint == NULL)
			{
				continue;
			}
			
			bool myDoIgnore = false;
			for (int i = 0; i < _myIgnores.size(); ++i)
			{
				Point2f* myIgnorePoint = &(_myIgnores.at(i));
				if (myPoint->distanceSquared(myIgnorePoint) <= (16 * _myLineThresholdSquared))
				{
					myDoIgnore = true;
					break;
				}
			}
			if (myDoIgnore)
				continue;
			
			mySupports.clear();
			
			
			for(int i = 0; i < _myRemainingLines.size(); i++){
				Segment* myLine = _myRemainingLines.at(i);
				
				if(myLine->distanceToPoint(myPoint) <= _myLineThreshold){
					mySupports.push_back(myLine);
				};
				
			}
			
			
			if (mySupports.size() > (mySupports2.size()))
			{
				mySupports2 = mySupports;
				myBestPoint = myPoint;
			}
		}
		if (myBestPoint != NULL)
		{
			_myVPProposal.x = myBestPoint->x;
			_myVPProposal.y = myBestPoint->y;
			
			
			// Add all
			for(int i = 0; i < mySupports2.size(); i++){
				_mySupportLines.push_back(mySupports2.at(i));;
			}
			
			_myHasProposal = true;
			return;
		}
		else
		{
			_myHasProposal = false;
			_myHasNoMoreProposals = true;
			
			printf("### We are out of lines! Nice. \n ");
			return;
		}
		
	}
	
	
	void VanishingPointOracle::processImage(){
		
		
		/* Create intermediate image */
		ofxCvColorImage* myImage = new ofxCvColorImage;
		myImage->setUseTexture(false);
		myImage->allocate(_myOriginalImage.width, _myOriginalImage.height);
		myImage->setFromPixels(_myOriginalImage.getPixels(), _myOriginalImage.width, _myOriginalImage.height);
		
		IplImage* myFullImage = cvCreateImage(cvGetSize(myImage->getCvImage()), IPL_DEPTH_8U, 4);
		
		
		
#ifdef DO_BLUR
		IplImage* myBlurImage =  cvCreateImage(cvGetSize(myImage->getCvImage()), IPL_DEPTH_8U, 3);
		cvSmooth(myImage->getCvImage(), myBlurImage, CV_GAUSSIAN);
		
		cvCvtColor(myBlurImage, myFullImage, CV_RGB2RGBA);
		cvReleaseImage(&myBlurImage);
		
#else
		cvCvtColor(myImage->getCvImage(), myFullImage, CV_RGB2RGBA);
#endif
		
		
		/* Delete useless conversion shit */
		delete myImage;
		
		
		printf("IplImage has %d channels and is %d x %d \n", myFullImage->nChannels, myFullImage->width, myFullImage->height);
		
		
		
		
		/* Canny */
		IplImage* myEdgeImage = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		
#ifdef RGB_CANNY
		
		cvSetImageCOI(myFullImage, 1);
		IplImage* color1 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		IplImage* colore1 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		cvCopy(myFullImage, color1);
		cvCanny(color1, colore1, 60, 180, 3);
		
		
		cvSetImageCOI(myFullImage, 2);
		IplImage* color2 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		IplImage* colore2 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		cvCopy(myFullImage, color2);
		cvCanny(color2, colore2, 60, 180,  3);
		
		
		cvSetImageCOI(myFullImage, 3);
		IplImage* color3 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		IplImage* colore3 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		
		cvCopy(myFullImage, color3);
		cvCanny(color3, colore3,60, 180, 3);
		
		
		IplImage* result1 = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		
		
		cvOr(colore1, colore2, result1);
		cvOr(colore2, result1, myEdgeImage);
		
		cvReleaseImage(&color1);
		cvReleaseImage(&color2);
		cvReleaseImage(&color3);
		
		cvReleaseImage(&colore1);
		cvReleaseImage(&colore2);
		cvReleaseImage(&colore3);
		
		cvReleaseImage(&result1);
		
		
		
#else
		/* Convert to grayscale for edge detection */
		cvCvtColor(myFullImage, myGrayImage, CV_RGBA2GRAY);
		
		IplImage* myGrayImage = cvCreateImage(cvGetSize(myFullImage), IPL_DEPTH_8U, 1);
		
		
		/* Perform edge detection */
		cvCanny(myGrayImage, myEdgeImage, 50, 150, 3);
		
		cvReleaseImage(&myGrayImage);
		
#endif		
		
		/* Hough transform */
		CvMemStorage* myStorage = cvCreateMemStorage(0);
		
		
		
		CvSeq* mySequence = cvHoughLines2(myEdgeImage, myStorage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 100, 27 * 2, 1 * 1);
		printf("Found %d lines \n", mySequence->total);
		
		
		
		
		
		
		/* Store Lines */		
		for(int i = 0; i < mySequence->total; i++){
			
			int* myCvLine = (int*) cvGetSeqElem( mySequence , i );
			
			int myX1 = myCvLine[0] ; // first int of first cvPoint struct
			int myY1 = myCvLine[1] ;
			int myX2 = myCvLine[2] ; // first int of second cvPoint struct
			int myY2 = myCvLine[3] ;
			
			if(myX1 == myX2 && myY1 == myY2) printf("FUCKFZUCKFUCK\n"); 
			
			
			/* Store line object & filter */
			Segment myLine(myX1,myY1,myX2,myY2);
			
			//if(!filterLine(myLine.angle())){
				
				_myLines.push_back(myLine);	
				
			//}
			
			
		}
		
		
		for(int i = 0; i < _myLines.size(); i++){
			
			_myRemainingLines.push_back(&_myLines.at(i));
		}
		
		
		
		
		
		
		/* Free OpenCV allocations */
		cvReleaseImage(&myEdgeImage);
		cvReleaseImage(&myFullImage);
		
		//delete mySaveImage;
		//delete myOriginal;
		
		
		
		
		
		cvReleaseMemStorage(&myStorage);
		
		
	}
	
	
	void VanishingPointOracle::loadImage(){
		/* Load image */
		_myOriginalImage.setUseTexture(false);
		_myOriginalImage.loadImage(_myPathName);
		_myOriginalImage.setImageType(OF_IMAGE_COLOR); // catch those damn grayscale fuckers
		
		
		printf("*** Loaded image: %d x %d \n",  _myOriginalImage.width, _myOriginalImage.height);
		std::cout <<  _myFileName << "\n";
	}
	
	
	bool VanishingPointOracle::filterLine(double theTheta){
		bool myDoFilter = false;
		double myTolerance = 0.02;
		
		
		// Atan2 ranges [-PI, PI]
		
		float myFlippedTheta =theTheta + CV_PI;
		
		if(myFlippedTheta + myTolerance > 0 && myFlippedTheta  - myTolerance <  0 ) myDoFilter = true;
		if(myFlippedTheta + myTolerance > CV_PI*0.5 && myFlippedTheta  - myTolerance < CV_PI*0.5) myDoFilter = true;
		if(myFlippedTheta + myTolerance > CV_PI && myFlippedTheta  - myTolerance < CV_PI) myDoFilter = true;
		if(myFlippedTheta + myTolerance > CV_PI*1.5 && myFlippedTheta  - myTolerance <  CV_PI*1.5 ) myDoFilter = true;		
		if(myFlippedTheta + myTolerance > CV_PI*2.0 && myFlippedTheta  - myTolerance <  CV_PI*2.0 ) myDoFilter = true;		
		
		
		return myDoFilter;
	}
	
	
	int VanishingPointOracle::randomInt(int theMaximum){
		int theValue = ofRandom(0, theMaximum -1);
		return theValue;
	}
	
	
	int VanishingPointOracle::getWidth(){
		return _myOriginalImage.getWidth();
	}	
	
	int VanishingPointOracle::getHeight(){
		return _myOriginalImage.getHeight();
	}
	
	
	void VanishingPointOracle::zoomIn(){
		_myScale *= 1.5;
	}
	
	void VanishingPointOracle::zoomOut(){
		_myScale /= 1.5;
	}
	
	std::string VanishingPointOracle::getFileName(){
		return _myFileName;
	}
	
	
	std::vector<VanishingPoint> VanishingPointOracle::getVanishingPoints(){
		return _mySelection;
	}

	
}